#include "WProgram.h"
#include "Road.h"
#include "WayPoint.h"

Road::Road()
{
}

/**
* TODO : Doc
*/
Road::Road(WayPoint wp1, WayPoint wp2)
{
 _wp1._latitude=wp1._latitude;
 _wp1._longitude=wp1._longitude;
 _wp2._latitude=wp2._latitude;
 _wp2._longitude=wp2._longitude;
}

/**
* If AB is the road, C a waypoint, returns the shortest distance between C and AB
*/
float Road::dist(WayPoint wp)
{
 Road temp(_wp1,wp);
 // Serial.print("temp.asVect()._x,temp.asVect()._y= "); //debug
 // Serial.print(temp.asVect()._x); // debug
 // Serial.print(", "); // debug
 // Serial.println(temp.asVect()._y); // debug
 return asVect().normedVect().det(temp.asVect());
}

/**
* TODO: Doc
*/
Road::Road(float latitude1, float longitude1, float latitude2, float longitude2)
{
 _wp1._latitude=latitude1;
 _wp1._longitude=longitude1;
 _wp2._latitude=latitude2;
 _wp2._longitude=longitude2;
}

float Road::length()
{
 return _wp1.dist(_wp2);
}

/**
* TODO : Doc
*/
Vector2D Road::asVect()
{
 Vector2D a(_wp1._longitude,_wp1._latitude);
 Vector2D b(_wp2._longitude,_wp2._latitude);
 Vector2D nDiff=b.minus(a).normedVect();
 return nDiff.mult(length());
}
